#include "w25qxx.h"
#include "quadspi.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#if 1
#define __INFO(format,...) printf(""format"", ##__VA_ARGS__) 
#else
#define __INFO(format,...) 
#endif

int w25qxx_qspi_reset(void)
{
    QSPI_CommandTypeDef s_command;
    int ret;
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = ENABLE_RESET;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if(ret != HAL_OK) return ret;
    s_command.Instruction = RESET_DEVICE;
    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    return ret;
}

unsigned short w25qxx_qspi_id(void)
{
    QSPI_CommandTypeDef s_command;
    unsigned char buf[2];
    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = READ_DEVICE_ID;
    s_command.AddressMode = QSPI_ADDRESS_1_LINE;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = 0;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.NbData = 2;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    HAL_QSPI_Receive(&hqspi, buf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    return (uint16_t)((buf[0] << 8) | buf[1]);
}

int w25qxx_qspi_init(void)
{
    unsigned short id;
    if (w25qxx_qspi_reset()) 
    {
        __INFO("w25qxx_qspi reset faild.\n");
        return -1;
    }
    HAL_Delay(100);
    id = w25qxx_qspi_id();
    if ((id & 0xFF00) != 0xEF00) return -1;
    __INFO("QSPI FLASH Device ID: %04X\n", id);
    return 0;
}

int w25qxx_qspi_write_enable(void)
{
    QSPI_CommandTypeDef s_command;
    QSPI_AutoPollingTypeDef s_config;
    int ret;

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = WRITE_ENABLE;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;

    s_command.Instruction = READ_SR1;
    s_command.DataMode = QSPI_DATA_1_LINE;

    s_config.Match           = 0x02;
    s_config.Mask            = 0x02;
    s_config.MatchMode       = QSPI_MATCH_MODE_AND;
    s_config.StatusBytesSize = 1;
    s_config.Interval        = 0x10;
    s_config.AutomaticStop   = QSPI_AUTOMATIC_STOP_ENABLE;

    ret = HAL_QSPI_AutoPolling(&hqspi, &s_command, &s_config, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;
    return 0;
}

int w25qxx_qspi_write_disable(void)
{
    QSPI_CommandTypeDef s_command;
    QSPI_AutoPollingTypeDef s_config;
    int ret;

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = WRITE_DISABLE;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;

    s_command.Instruction = READ_SR1;
    s_command.DataMode = QSPI_DATA_1_LINE;

    s_config.Match           = 0x00;
    s_config.Mask            = 0x02;
    s_config.MatchMode       = QSPI_MATCH_MODE_AND;
    s_config.StatusBytesSize = 1;
    s_config.Interval        = 0x10;
    s_config.AutomaticStop   = QSPI_AUTOMATIC_STOP_ENABLE;

    ret = HAL_QSPI_AutoPolling(&hqspi, &s_command, &s_config, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;
    return 0;
}

int w25qxx_qspi_wait_busy(void)
{
    QSPI_CommandTypeDef s_command;
    QSPI_AutoPollingTypeDef s_config;
    int ret;

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = READ_SR1;
    s_command.AddressMode = QSPI_ADDRESS_NONE;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    s_config.Match           = 0x00;
    s_config.Mask            = 0x01;
    s_config.MatchMode       = QSPI_MATCH_MODE_AND;
    s_config.StatusBytesSize = 1;
    s_config.Interval        = 0x10;
    s_config.AutomaticStop   = QSPI_AUTOMATIC_STOP_ENABLE;

    ret = HAL_QSPI_AutoPolling(&hqspi, &s_command, &s_config, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;
    return 0;
}

int w25qxx_qspi_erase(unsigned int address)
{
    QSPI_CommandTypeDef s_command;
    int ret;

    if (w25qxx_qspi_write_enable()) 
    {
        __INFO("w25qxx_qspi_write_enable faild.\n");
        return -1;
    }

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = SECTOR_ERASE;
    s_command.AddressMode = QSPI_ADDRESS_1_LINE;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = address;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_NONE;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return -1;

    if (w25qxx_qspi_wait_busy())
    {
        __INFO("w25qxx_qspi_wait_busy faild.\n");
        return -1;
    }

    return 0;
}

int w25qxx_qspi_read(unsigned int address, void *pbuf, int size)
{
    QSPI_CommandTypeDef s_command;
    int ret;

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = READ_DATA;
    s_command.AddressMode = QSPI_ADDRESS_1_LINE;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = address;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.NbData = size;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return 0;

    ret = HAL_QSPI_Receive(&hqspi, (unsigned char *)pbuf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return 0;

    return size;
}

int w25qxx_qspi_page_program(unsigned int address, void *pbuf, int size)
{
    QSPI_CommandTypeDef s_command;
    int ret;

    if (w25qxx_qspi_write_enable()) 
    {
        __INFO("w25qxx_qspi_write_enable faild.\n");
        return 0;
    }

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = PAGE_PROGRAM;
    s_command.AddressMode = QSPI_ADDRESS_1_LINE;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = address;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
    s_command.DummyCycles = 0;
    s_command.DataMode = QSPI_DATA_1_LINE;
    s_command.NbData = size;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    ret = HAL_QSPI_Command(&hqspi, &s_command, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return 0;
    ret = HAL_QSPI_Transmit(&hqspi, pbuf, HAL_QPSI_TIMEOUT_DEFAULT_VALUE);
    if (ret != HAL_OK) return 0;
    
    if (w25qxx_qspi_wait_busy())
    {
        __INFO("w25qxx_qspi_wait_busy faild.\n");
        return 0;
    }

    return size;
}

int w25qxx_qspi_write_directly(unsigned int address, void *pbuf, int size)
{
    unsigned char *pdata;
    int write_size, remain;
    int ret;

    if (pbuf == NULL) return 0;
    pdata = (unsigned char *)pbuf;
    remain = size;

    while (remain > 0)
    {
        if (address % W25QXX_PAGE_SIZE)
            write_size = W25QXX_PAGE_SIZE - address % W25QXX_PAGE_SIZE;
        else 
            write_size = W25QXX_PAGE_SIZE;
        
        if(remain < write_size) 
            write_size = remain;

        ret = w25qxx_qspi_page_program(address, pdata, write_size);
        if (ret != write_size) break;

        remain -= write_size;
        if (!remain) break;
        pdata += write_size;
        address += write_size;
    }
    return (int)(size - remain);
}

int w25qxx_qspi_write(unsigned int address, void *pbuf, int size)
{
    unsigned char *pdata;
    int write_size, remain;
    unsigned char buffer[W25QXX_SECTOR_SIZE];
    int ret;

    if (pbuf == NULL) return 0;
    pdata = (unsigned char *)pbuf;
    remain = size;
    while (remain > 0)
    {
        if (address % W25QXX_SECTOR_SIZE)
            write_size = W25QXX_SECTOR_SIZE - address % W25QXX_SECTOR_SIZE;
        else
            write_size = W25QXX_SECTOR_SIZE;

        if (remain < write_size) write_size = remain;

        if (write_size != W25QXX_SECTOR_SIZE)
        {
            ret = w25qxx_spi_read(address - address % W25QXX_SECTOR_SIZE, buffer, W25QXX_SECTOR_SIZE);
            if (ret != W25QXX_SECTOR_SIZE) break;
        }
        memcpy(&buffer[address % W25QXX_SECTOR_SIZE], pdata, write_size);
        ret = w25qxx_qspi_erase(address - address % W25QXX_SECTOR_SIZE);
        if (ret != 0) break;
        ret = w25qxx_qspi_write_directly(address - address % W25QXX_SECTOR_SIZE, buffer, W25QXX_SECTOR_SIZE);
        if (ret != W25QXX_SECTOR_SIZE) break;

        remain -= write_size;
        if(!remain) break;
        pdata += write_size;
        address += write_size;
    }
    return (int)(size - remain);
}

int w25qxx_qspi_mem_mapped(void)
{
    QSPI_CommandTypeDef s_command;
    QSPI_MemoryMappedTypeDef s_mem_mapped_cfg;
    int ret;

    s_command.InstructionMode = QSPI_INSTRUCTION_1_LINE;
    s_command.Instruction = FAST_READ_QUAD;
    s_command.AddressMode = QSPI_ADDRESS_4_LINES;
    s_command.AddressSize = QSPI_ADDRESS_24_BITS;
    s_command.Address = 0;
    s_command.AlternateByteMode = QSPI_ALTERNATE_BYTES_4_LINES;
    s_command.AlternateBytesSize = QSPI_ALTERNATE_BYTES_8_BITS;
    s_command.AlternateBytes = 0xFF;
    s_command.DummyCycles = 4;
    s_command.DataMode = QSPI_DATA_4_LINES;
    s_command.DdrMode = QSPI_DDR_MODE_DISABLE;
    s_command.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
    s_command.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;

    s_mem_mapped_cfg.TimeOutActivation = QSPI_TIMEOUT_COUNTER_DISABLE;
    s_mem_mapped_cfg.TimeOutPeriod = 0;

    ret = HAL_QSPI_MemoryMapped(&hqspi, &s_command, &s_mem_mapped_cfg);
    if (ret != HAL_OK) return -1;
    return 0;
}
